function m_a = rodrigues(a,q)
E = eye(3);
m_a = E + matHat0(a)*sin(q) + matHat0(a)^2.*(1-cos(q));
end

function temp=matHat0(a)
temp=[0       -a(3,1)  a(2,1);
      a(1,1)  0       -a(1,1);
     -a(2,1)  a(1,1)   0    ];
end